三维空间点拟合圆原理及C++实现

已知三维空间离散点坐标(xi, yi, zi)，构建一个空间圆使得空间点尽可能靠近拟合的空间圆。

首先，所有离散点尽可能在一个平面上，平面方程可表示为

                                                                                                             (1)

写成矩阵形式为，

，式中

，，                                              (2)

这是一个超定方程求解，根据最小二乘法，可以求出，即平面的法向向量。

假设所有离散点都在圆上，那么任意两点的连线的中垂线必过圆心。设圆心C（x0，y0，z0），取两个点P1（x1，y1，z1）与P2（x2，y2，z2），则P1和P2连线的向量vector1表示为（x2-x1，y2-y1，z2-z1），P1和P2连线的中点坐标P12为

圆心C与P12连线向量vector2为。要想P1与P2在圆上，则满足vector1*vecotor2=0，即

整理一下，有

,

式中  ,

所有点都在圆上，则有

                                         (3)

写成矩阵形式   ，

式中，，

上述方程亦为超定方程，变换成适定形式为

                                                                                                    (4)

由于圆心C必在前述控制的平面内，因此满足, 即 

                                                                                                                  (5)

A为平面的法向量，通过   （6）已求出，因而可将式(4)和式(5)合并，写成一个扩展式进行求解

，式中

，

则求解圆心坐标

圆的半径可由所有点到圆心的距离的平均值确定：

C++代码实现如下：

vector<float> FitCircle(std::vector<vector<float>> pts)
{
    vector<float> circle;

    int num = pts.size();
    int dim = 3;

    Eigen::MatrixXd M(num, dim);

    for (int i = 0; i < num; i++)
    {
        for (int j = 0; j < dim; j++)
        {
            M(i, j) = pts[i][j];
        }
    }

    Eigen::MatrixXd L1 = Eigen::MatrixXd::Ones(num, 1);

    //式（6）
    Eigen::MatrixXd A = (M.transpose()*M).inverse()*M.transpose()*L1;

    Eigen::MatrixXd B = Eigen::MatrixXd::Zero(num - 1, 3);

    for (int i = 0; i < num - 1; i++)
    {
        B.row(i) = M.row(i + 1) - M.row(i);
    }

    Eigen::MatrixXd L2 = Eigen::MatrixXd::Zero(num - 1, 1);
    for (int i = 0; i < num - 1; i++)
    {
        L2(i) = (M(i + 1, 0)*M(i + 1, 0) + M(i + 1, 1)*M(i + 1, 1) + M(i + 1, 2)*M(i + 1, 2)
            - (M(i, 0)*M(i, 0) + M(i, 1)*M(i, 1) + M(i, 2)*M(i, 2))) / 2.0;
    }

    Eigen::MatrixXd D;
    //！！！矩阵合并前需要将合并后的矩阵 resize
    D.resize(4, 3);
    D << B.transpose()*B,
        A.transpose();

    Eigen::MatrixXd L3;
    Eigen::MatrixXd One31 = Eigen::MatrixXd::Ones(3, 1);
    L3.resize(4, 1);
    L3 << B.transpose()*L2,
        One31;
    
    //式（7）
    Eigen::MatrixXd C = (D.transpose()*D).inverse()*D.transpose() * L3;

    //式（8）
    double radius = 0;
    for (int i = 0; i < num; i++)
    {
        Eigen::MatrixXd tmp = M.row(i) - C.transpose();
        radius = radius + sqrt(tmp(0)*tmp(0) + tmp(1)*tmp(1) + tmp(2)*tmp(2));
    }
    radius = radius / num;

    circle.push_back(C(0));
    circle.push_back(C(1));
    circle.push_back(C(2));
    circle.push_back(A(0));
    circle.push_back(A(1));
    circle.push_back(A(2));
    circle.push_back(radius);
    return circle;
}